/*============================================================================*/
/*  Copyright (C) 2009-2011, iSOFT INFRASTRUCTURE SOFTWARE CO.,LTD.
 *  
 *  All rights reserved. This software is iSOFT property. Duplication 
 *  or disclosure without iSOFT written authorization is prohibited.
 *  
 *  @file       <Xcp_Cal.c>
 *  @brief      <Xcp Module CAL C file, including all function associated with
 *              online calibration and page switching.>
 *  
 *  <Compiler: Codewarrior for mpc55xx v2.6     MCU:MPC56XX>
 *  
 *  @author     <DY.Sun, QT.Yu>
 *  @date       <2013-03-06>
 */
/*============================================================================*/

/*=======[R E V I S I O N   H I S T O R Y]====================================*/
/*  <VERSION>    <DATE>    <AUTHOR>         <REVISION LOG>
 *  V1.0.0       20130306  DY.Sun, QT.Yu     Initial version
 * 
 */
/*============================================================================*/

/*=======[V E R S I O N  I N F O R M A T I O N]===============================*/
#define XCP_CAL_C_AR_MAJOR_VERSION  4
#define XCP_CAL_C_AR_MINOR_VERSION  0
#define XCP_CAL_C_AR_PATCH_VERSION  3
#define XCP_CAL_C_SW_MAJOR_VERSION  1
#define XCP_CAL_C_SW_MINOR_VERSION  0
#define XCP_CAL_C_SW_PATCH_VERSION  0

/*=======[I N C L U D E S]====================================================*/
#include "Xcp.h"
#include "Xcp_Internal.h"

/*=======[V E R S I O N  C H E C K]===========================================*/
#if (XCP_CAL_C_AR_MAJOR_VERSION != XCP_H_AR_MAJOR_VERSION)
  #error "Xcp_Cal.c : Mismatch in Specification Major Version"
#endif
#if (XCP_CAL_C_AR_MINOR_VERSION != XCP_H_AR_MINOR_VERSION)
  #error "Xcp_Cal.c : Mismatch in Specification Major Version"
#endif
#if (XCP_CAL_C_AR_PATCH_VERSION != XCP_H_AR_PATCH_VERSION)
  #error "Xcp_Cal.c : Mismatch in Specification Major Version"
#endif
#if (XCP_CAL_C_SW_MAJOR_VERSION != XCP_H_SW_MAJOR_VERSION)
  #error "Xcp_Cal.c : Mismatch in Specification Major Version"
#endif
#if (XCP_CAL_C_SW_MINOR_VERSION != XCP_H_SW_MINOR_VERSION)
  #error "Xcp_Cal.c : Mismatch in Specification Major Version"
#endif
#if (XCP_CAL_C_SW_PATCH_VERSION != XCP_H_SW_PATCH_VERSION)
  #error "Xcp_Cal.c : Mismatch in Specification Major Version"
#endif

#if (XCP_PL_CAL == (XCP_PL_CAL & XCP_RESOURCE))

/*=======[M A C R O S]========================================================*/
#define XCP_MODIFY_BITS_MAX_S   32
#define XCP_SET_PAG_MODE_ALL    0x83u
#define XCP_SET_PAG_MODE_ECU    0x01u
#define XCP_SET_PAG_MODE_XCP    0x02u

#define XCP_GET_PAG_MODE_ECU    0x01u
#define XCP_GET_PAG_MODE_XCP    0x02u

#define XCP_MAS0_ESEL_Offset 		16u
#define XCP_MAS0_TLBSEL_Offset		28u
#define XCP_MAS1_VAILD_Offset		31u
#define XCP_MAS1_IPORT_Offset		30u  
#define XCP_MAS1_TID_Offset 		16u
#define XCP_MAS1_SIZE_Offest		8u
#define XCP_MAS2_EPN_Offest			12u
#define XCP_MAS3_RPN_Offest			12u
#define XCP_MAS1_VAILD_MASK         0x80000000u           


#define XCP_TLB_TLBSEL				1u
#define XCP_TLB_MAX_NUM			    16u


/*=======[I N T E R N A L   F U N C T I O N   D E C L A R A T I O N S]========*/
#if (XCP_PAG_SUPPORT == STD_ON)
STATIC FUNC(void, XCP_CODE) Xcp_SwitchToRam(void);
STATIC FUNC(void, XCP_CODE) Xcp_SwitchToFlash(void);

#if (XCP_MMU_SUPPORT == STD_ON)
STATIC FUNC(void, XCP_CODE) Xcp_GetSprMas1(uint32 *valP);
STATIC FUNC(void, XCP_CODE) Xcp_TLBSwitchPage(uint8 pid);
STATIC FUNC(void, XCP_CODE) Xcp_SetSprMas0(uint32 val);
STATIC FUNC(void, XCP_CODE) Xcp_SetSprMas1(uint32 val);
STATIC FUNC(void, XCP_CODE) Xcp_SetSprMas2(uint32 val);
STATIC FUNC(void, XCP_CODE) Xcp_SetSprMas3(uint32 val);
STATIC FUNC(void, XCP_CODE) Xcp_WriteTlb(void);
STATIC FUNC(void, XCP_CODE) Xcp_ReadTlb(void);

STATIC FUNC(void, XCP_CODE) Xcp_WriteTlbEntry
    (P2CONST(Xcp_TLBInfoType, AUTOMATIC, XCP_CONST_PBCFG) tlbInfo,uint8 tlbEntry);
STATIC FUNC(Std_ReturnType, XCP_CODE) Xcp_MmuInit(void);
#endif
#endif

/*
 * Complex Command Handler
 */
STATIC FUNC(void, XCP_CODE) Xcp_DownloadHal(void);
#if (XCP_PAG_SUPPORT == STD_ON)
#if (XCP_SET_CAL_PAGE == STD_ON)
STATIC FUNC(void, XCP_CODE) Xcp_SetCalPageHal(void);
#endif
#endif

/*=======[F U N C T I O N   I M P L E M E N T A T I O N S]====================*/

/******************************************************************************/
/*
 * @brief               <Calibration Initialization>
 * 
 * < > .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
FUNC(Std_ReturnType, XCP_CODE) Xcp_CALInit(void)
{

    uint8 segNum;
    
    #if (XCP_PAG_SUPPORT == STD_ON)
    #if (XCP_MMU_SUPPORT == STD_ON)
    if (E_NOT_OK == Xcp_MmuInit())
    {
        return E_NOT_OK;
    }
    else
    #endif
    #endif
    {
        /* init the RAM page */
        for (segNum = 0; segNum < Xcp_SegmentInfo.maxSegNum; segNum++)
        {
            
            Xcp_CalBufferCopy(Xcp_SegmentInfo.pageInfoPtr[segNum].romStart,
                              Xcp_SegmentInfo.pageInfoPtr[segNum].ramStart,
                              Xcp_SegmentInfo.pageInfoPtr[segNum].size);
            #if (XCP_PAG_SUPPORT == STD_ON)
            #if (XCP_MMU_SUPPORT == STD_OFF)
            Xcp_CalBufferCopy(Xcp_SegmentInfo.pageInfoPtr[segNum].ramStart,
                              Xcp_SegmentInfo.pageInfoPtr[segNum].bufferPtr,
                              Xcp_SegmentInfo.pageInfoPtr[segNum].size);
            Xcp_ActivPagNum[segNum] = XCP_RAM_PAGE_NUM;
			#endif
            #endif
        }
        #if (XCP_PAG_SUPPORT == STD_ON)
        #if (XCP_MMU_SUPPORT == STD_ON)
        Xcp_SwitchToRam();
        #endif
        #endif
        
        return E_OK;
    }
}


/*
 * CAL/PAG Mandatory Function
 */

STATIC FUNC(void, XCP_CODE) Xcp_DownloadHal(void)
{
    uint8 pos;
    uint8 numOfElements = Xcp_CmdBuffer[1u];
    P2VAR(Xcp_AGType, AUTOMATIC, XCP_VAR) sourPtr;
    P2VAR(Xcp_AGType, AUTOMATIC, XCP_VAR) destPtr;
    uint32 u4DestAddr  = Xcp_Mta2Ptr(Xcp_MTA.extension,Xcp_MTA.transferAddr);

    if (FALSE == Xcp_CheckAddress(u4DestAddr, (numOfElements*XCP_AG), XCP_MEM_CAL_RAM))
    {
        if (TRUE == Xcp_CheckAddress(u4DestAddr, (numOfElements*XCP_AG), XCP_MEM_CAL_ROM))
        {
            Xcp_SetErrorCode(XCP_ERR_WRITE_PROTECTED);
            Xcp_RespLength = 0x02u;  
        }
        else
        {
            Xcp_SetErrorCode(XCP_ERR_ACCESS_DENIED);
            Xcp_RespLength = 0x02u;  
        }
    }
    else
    {
        /* write the data of CRO to destination address. */
        sourPtr = (Xcp_AGType*)&Xcp_CmdBuffer[XCP_DOWNLOAD_DATA_OFFSET];
        destPtr = (Xcp_AGType*)u4DestAddr;
        for (pos = 0u; pos < numOfElements; pos++)
        {
            destPtr[pos] = sourPtr[pos];
        }
        /*updata the MTA*/
        Xcp_UpdateMTA(numOfElements*XCP_AG);
    }    
    return;
}

/******************************************************************************/
/*
 * @brief               <Command Download Handler>
 * 
 * < > .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
FUNC(void, XCP_CODE) Xcp_Download(void)
{    
    /* check frame length */
    #if ((STD_OFF == XCP_CAN_MAX_DLC_REQUIRED) && (STD_ON == XCP_ON_CAN_ENABLE))
    if (
    #if (XCP_ADDRESS_GRANULARITY != XCP_AG_DWORD)
    (Xcp_CmdLength != (2u +(Xcp_CmdBuffer[1u] * XCP_AG)))
    #else
    (Xcp_CmdLength != (4u +(Xcp_CmdBuffer[1u] * XCP_AG)))
    #endif
    && (Xcp_CmdLength != XCP_CAN_MAX_DLC))
    {
        Xcp_SetErrorCode(XCP_ERR_CMD_SYNTAX);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check pgm active status */
    #if (XCP_PL_PGM == (XCP_PL_PGM & XCP_RESOURCE))
    if (Xcp_PgmStauts != XCP_PGM_IDLE)
    {
        Xcp_SetErrorCode(XCP_ERR_PGM_ACTIVE);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check protection status */
    if (XCP_PL_CAL == (Xcp_ProtectionStatus & XCP_PL_CAL))
    {
        Xcp_SetErrorCode(XCP_ERR_ACCESS_LOCKED);
        Xcp_RespLength = 0x02u;
    }
    else if ((Xcp_CmdBuffer[1u] > XCP_DOWNLOAD_SIZE)
    || (0u == Xcp_CmdBuffer[1u]))
    {
        Xcp_SetErrorCode(XCP_ERR_OUT_OF_RANGE);
        Xcp_RespLength = 0x02u;  
    }
    else
    {
        Xcp_DownloadHal();
    }
    Xcp_SendResp();
    return;
}

/*
 * CAL/PAG Optional Function
 */
#if (XCP_DOWNLOAD_NEXT == STD_ON)
/******************************************************************************/
/*
 * @brief               <Command DownloadNext Handler>
 * 
 * < > .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
FUNC(void, XCP_CODE) Xcp_DownloadNext(void)
{
    uint8 pos;
    Xcp_AGType * sourPtr;
    Xcp_AGType * destPtr;
    uint32 u4DestAddr  = Xcp_Mta2Ptr(Xcp_MTA.extension,Xcp_MTA.transferAddr);  

    /* check frame length has been checked before */
    /* check pgm active status */
    #if (XCP_PL_PGM == (XCP_PL_PGM & XCP_RESOURCE))
    if (Xcp_PgmStauts != XCP_PGM_IDLE)
    {
        Xcp_SetErrorCode(XCP_ERR_PGM_ACTIVE);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check protection status */
    if (XCP_PL_CAL == (Xcp_ProtectionStatus & XCP_PL_CAL))
    {
        Xcp_SetErrorCode(XCP_ERR_ACCESS_LOCKED);
        Xcp_RespLength = 0x02u;
    }
    else if (FALSE == Xcp_CheckAddress(u4DestAddr, (Xcp_BlockBufferLen*XCP_AG), XCP_MEM_CAL_RAM))
    {
        if (TRUE == Xcp_CheckAddress(u4DestAddr, (Xcp_BlockBufferLen*XCP_AG), XCP_MEM_CAL_ROM))
        {
            Xcp_SetErrorCode(XCP_ERR_WRITE_PROTECTED);
            Xcp_RespLength = 0x02u;  
        }
        else
        {
            Xcp_SetErrorCode(XCP_ERR_ACCESS_DENIED);
            Xcp_RespLength = 0x02u;  
        }
    }
    else
    {
        /* write the data in BlockBuffer to destination address. */
        sourPtr = &Xcp_BlockBuffer[0u];
        destPtr = (Xcp_AGType*)u4DestAddr;
        for (pos = 0u; pos < Xcp_BlockBufferLen; pos++)
        {
            destPtr[pos] = sourPtr[pos];
        }
        /*updata the MTA*/
        Xcp_UpdateMTA(Xcp_BlockBufferLen*XCP_AG);
    }
    Xcp_SendResp();
    return;
}
#endif

#if (XCP_DOWNLOAD_MAX == STD_ON)
/******************************************************************************/
/*
 * @brief               <Command DownloadMax Handler>
 * 
 * < > .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
FUNC(void, XCP_CODE) Xcp_DownloadMax(void)
{
    uint8 pos;
    Xcp_AGType * sourPtr;
    Xcp_AGType * destPtr;
    uint32 u4DestAddr  = Xcp_Mta2Ptr(Xcp_MTA.extension,Xcp_MTA.transferAddr);  

    /* check frame length */
    #if ((STD_OFF == XCP_CAN_MAX_DLC_REQUIRED) && (STD_ON == XCP_ON_CAN_ENABLE))
    /* caution: if the bus interface is not CAN the length check here shall be modified */
    if (Xcp_CmdLength != XCP_CAN_MAX_DLC)
    {
        Xcp_SetErrorCode(XCP_ERR_CMD_SYNTAX);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif           
    /* check pgm active status */
    #if (XCP_PL_PGM == (XCP_PL_PGM & XCP_RESOURCE))
    if (Xcp_PgmStauts != XCP_PGM_IDLE)
    {
        Xcp_SetErrorCode(XCP_ERR_PGM_ACTIVE);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check protection status */
    if (XCP_PL_CAL == (Xcp_ProtectionStatus & XCP_PL_CAL))
    {
        Xcp_SetErrorCode(XCP_ERR_ACCESS_LOCKED);
        Xcp_RespLength = 0x02u;
    }
    else if (FALSE == Xcp_CheckAddress(u4DestAddr, (XCP_DOWNLOADMAX_SIZE*XCP_AG), XCP_MEM_CAL_RAM))
    {
        if (TRUE == Xcp_CheckAddress(u4DestAddr, (XCP_DOWNLOADMAX_SIZE*XCP_AG), XCP_MEM_CAL_ROM))
        {
            Xcp_SetErrorCode(XCP_ERR_WRITE_PROTECTED);
            Xcp_RespLength = 0x02u;  
        }
        else
        {
            Xcp_SetErrorCode(XCP_ERR_ACCESS_DENIED);
            Xcp_RespLength = 0x02u;  
        }
    }
    else
    {
        /* write the data of CRO to destination address. */
        sourPtr = (Xcp_AGType*)&Xcp_CmdBuffer[XCP_AG];
        destPtr = (Xcp_AGType*)u4DestAddr;
        for (pos = 0u; pos < XCP_DOWNLOADMAX_SIZE; pos++)
        {
            destPtr[pos] = sourPtr[pos];
        }
        /*updata the MTA*/
        Xcp_UpdateMTA(XCP_DOWNLOADMAX_SIZE*XCP_AG);
    }
    Xcp_SendResp();
    return;
}
#endif

#if (XCP_MODIFY_BITS == STD_ON)
/******************************************************************************/
/*
 * @brief               <Command ModifyBits Handler>
 * 
 * < > .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
FUNC(void, XCP_CODE) Xcp_ModifyBits(void)
{
    uint8 shiftValue = Xcp_CmdBuffer[1u];
    uint16 andMask;
    uint16 xorMask;
    uint32 u4SourceAddr = Xcp_Mta2Ptr(Xcp_MTA.extension,Xcp_MTA.transferAddr);  
    
    /* check frame length */
    #if ((STD_OFF == XCP_CAN_MAX_DLC_REQUIRED) && (STD_ON == XCP_ON_CAN_ENABLE))
    if ((Xcp_CmdLength != 0x06u)
    && (Xcp_CmdLength != XCP_CAN_MAX_DLC))
    {
        Xcp_SetErrorCode(XCP_ERR_CMD_SYNTAX);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check pgm active status */
    #if (XCP_PL_PGM == (XCP_PL_PGM & XCP_RESOURCE))
    if (Xcp_PgmStauts != XCP_PGM_IDLE)
    {
        Xcp_SetErrorCode(XCP_ERR_PGM_ACTIVE);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check protection status */
    if (XCP_PL_CAL == (Xcp_ProtectionStatus & XCP_PL_CAL))
    {
        Xcp_SetErrorCode(XCP_ERR_ACCESS_LOCKED);
        Xcp_RespLength = 0x02u;
    }
    else if (FALSE == Xcp_CheckAddress(u4SourceAddr, sizeof(uint32), XCP_MEM_CAL_RAM))
    {
        if (TRUE == Xcp_CheckAddress(u4SourceAddr, sizeof(uint32), XCP_MEM_CAL_ROM))
        {
            Xcp_SetErrorCode(XCP_ERR_WRITE_PROTECTED);
            Xcp_RespLength = 0x02u;  
        }
        else
        {
            Xcp_SetErrorCode(XCP_ERR_ACCESS_DENIED);
            Xcp_RespLength = 0x02u;  
        }
    }
    else if (shiftValue >= XCP_MODIFY_BITS_MAX_S)
    {
        Xcp_SetErrorCode(XCP_ERR_OUT_OF_RANGE);
        Xcp_RespLength = 0x02u;  
    }
    else
    {
        /* get infomation from the frame */
        Xcp_CopyU1BufferToU2(&Xcp_CmdBuffer[2], &andMask, CPU_BYTE_ORDER);
        Xcp_CopyU1BufferToU2(&Xcp_CmdBuffer[4], &xorMask, CPU_BYTE_ORDER);
        /* do modify */
        *(uint32*)u4SourceAddr &= (~((uint32)(((uint16)~andMask) << shiftValue)));
        *(uint32*)u4SourceAddr ^= ((uint32)(xorMask << shiftValue));
        /* Do not updata the MTA*/
    }
    Xcp_SendResp();
    return;
}
#endif

/*
 * PAG
 */
#if (XCP_PAG_SUPPORT == STD_ON)

#if (XCP_MMU_SUPPORT == STD_OFF)
/******************************************************************************/
/*
 * @brief               <Switching Page to RAM>
 * 
 * <Switch Page to RAM by using Buffer> .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
STATIC FUNC(void, XCP_CODE) Xcp_SwitchToRam(void)
{
    uint8 segNum;
    for (segNum = 0; segNum < Xcp_SegmentInfo.maxSegNum; segNum++)
    {
        Xcp_CalBufferCopy(Xcp_SegmentInfo.pageInfoPtr[segNum].bufferPtr,
                          Xcp_SegmentInfo.pageInfoPtr[segNum].ramStart,
                          Xcp_SegmentInfo.pageInfoPtr[segNum].size);
        Xcp_ActivPagNum[segNum] = XCP_RAM_PAGE_NUM;
    }
	return;	
}

/******************************************************************************/
/*
 * @brief               <Switching Page to FLASH>
 * 
 * <Switch Page to FLASH by using Buffer> .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
STATIC FUNC(void, XCP_CODE) Xcp_SwitchToFlash(void)
{
    uint8 segNum;
    for (segNum = 0; segNum < Xcp_SegmentInfo.maxSegNum; segNum++)
    {
        Xcp_CalBufferCopy(Xcp_SegmentInfo.pageInfoPtr[segNum].ramStart,
                          Xcp_SegmentInfo.pageInfoPtr[segNum].bufferPtr,
                          Xcp_SegmentInfo.pageInfoPtr[segNum].size);
        Xcp_CalBufferCopy(Xcp_SegmentInfo.pageInfoPtr[segNum].romStart,
                          Xcp_SegmentInfo.pageInfoPtr[segNum].ramStart,
                          Xcp_SegmentInfo.pageInfoPtr[segNum].size);
        Xcp_ActivPagNum[segNum] = XCP_FLASH_PAGE_NUM;
    }
	return;	
}
/* MPC5634M MMU solution */
#else
/******************************************************************************/
/*
 * @brief               <Get MAS1 from SP Register>
 * 
 * <Get MAS1 from SP Register> .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
STATIC FUNC(void, XCP_CODE) Xcp_GetSprMas1(uint32 *valP)
{
	asm volatile (" mfspr %0,625"  : "=r"(*valP): );
	return;
}

/******************************************************************************/
/*
 * @brief               <Do Page Switching>
 * 
 * <Do Page Switching> .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
STATIC FUNC(void, XCP_CODE) Xcp_TLBSwitchPage(uint8 pid)
{
	asm volatile (" mtspr 48,%[_val]" : : [_val] "r" (pid));
	return;
}

STATIC FUNC(void, XCP_CODE) Xcp_SetSprMas0(uint32 val)
{
    asm volatile (" mtspr 624,%[_val]" : : [_val] "r" (val));
    return;
}

STATIC FUNC(void, XCP_CODE) Xcp_SetSprMas1(uint32 val)
{
    asm volatile (" mtspr 625,%[_val]" : : [_val] "r" (val));
    return;
}

STATIC FUNC(void, XCP_CODE) Xcp_SetSprMas2(uint32 val)
{
    asm volatile (" mtspr 626,%[_val]" : : [_val] "r" (val));
    return;
}

STATIC FUNC(void, XCP_CODE) Xcp_SetSprMas3(uint32 val)
{
    asm volatile (" mtspr 627,%[_val]" : : [_val] "r" (val));
    return;
}

STATIC FUNC(void, XCP_CODE) Xcp_WriteTlb(void)
{
	asm volatile (" tlbwe");
    return;
}

STATIC FUNC(void, XCP_CODE) Xcp_ReadTlb(void)
{
	asm volatile (" tlbre");
    return;
}

/******************************************************************************/
/*
 * @brief               <Write PID Info into the TLB>
 * 
 * <Write PID Info into the TLB> .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
STATIC FUNC(void, XCP_CODE) Xcp_WriteTlbEntry
    (P2CONST(Xcp_TLBInfoType, AUTOMATIC, XCP_CONST_PBCFG) tlbInfo,uint8 tlbEntry)
{
	uint32 val = 0;	
	val = ((uint32)XCP_TLB_TLBSEL<<XCP_MAS0_TLBSEL_Offset)|((uint32)tlbEntry<<XCP_MAS0_ESEL_Offset);
    Xcp_SetSprMas0(val);
	val = ((uint32)(tlbInfo->vaild)<<XCP_MAS1_IPORT_Offset)|
				((uint32)(tlbInfo->tid)<<XCP_MAS1_TID_Offset)|
				((uint32)(tlbInfo->size)<<XCP_MAS1_SIZE_Offest);
    Xcp_SetSprMas1(val);
	val = ((uint32)(tlbInfo->epn)<<XCP_MAS2_EPN_Offest)|
	    		((uint32)(tlbInfo->mas2flag));
    Xcp_SetSprMas2(val);
	val = ((uint32)(tlbInfo->rpn)<<XCP_MAS3_RPN_Offest)|
				((uint32)0x3f);
    Xcp_SetSprMas3(val);
    Xcp_WriteTlb();
    return;
}

/******************************************************************************/
/*
 * @brief               <MMU module Initialization>
 * 
 * <Initialize the MMU mode for page switching and than switch to the RP> .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
STATIC FUNC(Std_ReturnType, XCP_CODE) Xcp_MmuInit(void)
{
	uint32 val = 0;
	uint16 entryFreeFlag = 0xffffu;
	uint8 entryUsedCnt = 0;
	uint8 segmentCnt;
    uint8 entryCnt;
    Std_ReturnType result = E_OK;
    
	/*check which entry has been used*/
	for (entryCnt = 0; entryCnt < XCP_TLB_MAX_NUM; entryCnt++)
	{
		val = ((uint32)XCP_TLB_TLBSEL<<XCP_MAS0_TLBSEL_Offset)|((uint32)entryCnt<<XCP_MAS0_ESEL_Offset);
        Xcp_SetSprMas0(val);
		Xcp_ReadTlb();
		Xcp_GetSprMas1(&val);
		if (XCP_MAS1_VAILD_MASK == (val & XCP_MAS1_VAILD_MASK))
		{
			entryUsedCnt++;
			entryFreeFlag &= ~(0x0001u<<entryCnt);
		}
	}
    if ((entryUsedCnt + Xcp_SegmentInfo.maxTlbNum) <= XCP_TLB_MAX_NUM)
    {
    	segmentCnt = 0;
    	for (entryCnt = 0; entryCnt < XCP_TLB_MAX_NUM; entryCnt++)
    	{
    		if ((entryFreeFlag &(0x0001u<<entryCnt))==(0x0001u<<entryCnt))
    		{
    			Xcp_WriteTlbEntry(&(Xcp_SegmentInfo.tlbInfoPtr[segmentCnt]),entryCnt);
    			entryFreeFlag &= ~(0x0001u<<entryCnt);
    			segmentCnt++;
    			if (segmentCnt == Xcp_SegmentInfo.maxTlbNum)
    			{
    			    /* for RAM hasn't been initlized yet, turn to FLASH first */
                    Xcp_TLBSwitchPage(XCP_FLASH_PAGE_PID);
    				break;
    			}
    		}
    	}
        if (segmentCnt != Xcp_SegmentInfo.maxTlbNum)
        {
            result = E_NOT_OK;
        }
    }
    else
    {
        result = E_NOT_OK;
    }
	return result;
}

/******************************************************************************/
/*
 * @brief               <Switching Page to RAM>
 * 
 * <Switch Page to RAM by using MMU> .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
STATIC FUNC(void, XCP_CODE) Xcp_SwitchToRam(void)
{
    uint8 segNum;

    Xcp_TLBSwitchPage(XCP_RAM_PAGE_PID);
    for (segNum = 0; segNum < Xcp_SegmentInfo.maxSegNum; segNum++)
    {
        Xcp_ActivPagNum[segNum] = XCP_RAM_PAGE_NUM;
    }
    return;	
}

/******************************************************************************/
/*
 * @brief               <Switching Page to Flash>
 * 
 * <Switch Page to Flash by using MMU> .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
STATIC FUNC(void, XCP_CODE) Xcp_SwitchToFlash(void)
{
    uint8 segNum;

    Xcp_TLBSwitchPage(XCP_FLASH_PAGE_PID);
    for (segNum = 0; segNum < Xcp_SegmentInfo.maxSegNum; segNum++)
    {
        Xcp_ActivPagNum[segNum] = XCP_FLASH_PAGE_NUM;
    }
	return;	
}
#endif

#if (XCP_SET_CAL_PAGE == STD_ON)
STATIC FUNC(void, XCP_CODE) Xcp_SetCalPageHal(void)
{
    uint8 tMode = Xcp_CmdBuffer[1];
    uint8 segNum = Xcp_CmdBuffer[2];
    uint8 pagNum = Xcp_CmdBuffer[3];
    
    if (tMode != XCP_SET_PAG_MODE_ALL)
    {
        Xcp_SetErrorCode(XCP_ERR_MODE_NOT_VALID);
        Xcp_RespLength = 0x02u;
    }
    else if (segNum >= Xcp_SegmentInfo.maxSegNum)
    {
        Xcp_SetErrorCode(XCP_ERR_SEGMENT_NOT_VALID);
        Xcp_RespLength = 0x02u;
    }
    else
    {
    	/* CANape always send pagNum = 0 to XCP.*/
		#if 0
        if (Xcp_ActivPagNum[segNum] != pagNum)
        {
            switch (pagNum)
            {
                case XCP_FLASH_PAGE_NUM:
                {
                    Xcp_SwitchToFlash();
                    break;
                }
                case XCP_RAM_PAGE_NUM:
                {
                    Xcp_SwitchToRam();
                    break;
                }
                default:
                {
                    Xcp_SetErrorCode(XCP_ERR_PAGE_NOT_VALID);
                    Xcp_RespLength = 0x02u;
                    break;
                }
            }
        }
		#else
        if(Xcp_ActivPagNum[segNum] !=  XCP_FLASH_PAGE_NUM && Xcp_ActivPagNum[segNum] !=  XCP_RAM_PAGE_NUM)
        {
			Xcp_SetErrorCode(XCP_ERR_PAGE_NOT_VALID);
			Xcp_RespLength = 0x02u;
		}
        else if(Xcp_ActivPagNum[segNum] ==  XCP_FLASH_PAGE_NUM)
        	Xcp_SwitchToRam();
        else
        	Xcp_SwitchToFlash();
		#endif
    }
    return;
}

/******************************************************************************/
/*
 * @brief               <Command SetCalPage Handler>
 * 
 * < > .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
FUNC(void, XCP_CODE) Xcp_SetCalPage(void)
{
    
    /* check frame length */
    #if ((STD_OFF == XCP_CAN_MAX_DLC_REQUIRED) && (STD_ON == XCP_ON_CAN_ENABLE))
    if ((Xcp_CmdLength != 0x04u)
    && (Xcp_CmdLength != XCP_CAN_MAX_DLC))
    {
        Xcp_SetErrorCode(XCP_ERR_CMD_SYNTAX);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check pgm active status */
    #if (XCP_PL_PGM == (XCP_PL_PGM & XCP_RESOURCE))
    if (Xcp_PgmStauts != XCP_PGM_IDLE)
    {
        Xcp_SetErrorCode(XCP_ERR_PGM_ACTIVE);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check protection status */
    if (XCP_PL_CAL == (Xcp_ProtectionStatus & XCP_PL_CAL))
    {
        Xcp_SetErrorCode(XCP_ERR_ACCESS_LOCKED);
        Xcp_RespLength = 0x02u;
    }
    /* according to the solution of CANape, page switching shall 
     * be excuted only in 0x83 mode.
     */
    else
    {
        Xcp_SetCalPageHal();
    }
    Xcp_SendResp();
    return;
}
#endif

#if (XCP_GET_CAL_PAGE == STD_ON)
/******************************************************************************/
/*
 * @brief               <Command getCalPage Handler>
 * 
 * < > .
 * Service ID   :       <None>
 * Sync/Async   :       <Synchronous>
 * Reentrancy           <Not Reentrant>
 * @param[in]           <None>
 * @param[out]          <None>
 * @param[in/out]       <None>
 * @return              <None> 
 */
/******************************************************************************/
FUNC(void, XCP_CODE) Xcp_GetCalPage(void)
{
    uint8 accessMode = Xcp_CmdBuffer[1];
    uint8 segNum = Xcp_CmdBuffer[2];
    /* check frame length */
    #if ((STD_OFF == XCP_CAN_MAX_DLC_REQUIRED) && (STD_ON == XCP_ON_CAN_ENABLE))
    if ((Xcp_CmdLength != 0x03u)
    && (Xcp_CmdLength != XCP_CAN_MAX_DLC))
    {
        Xcp_SetErrorCode(XCP_ERR_CMD_SYNTAX);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check pgm active status */
    #if (XCP_PL_PGM == (XCP_PL_PGM & XCP_RESOURCE))
    if (Xcp_PgmStauts != XCP_PGM_IDLE)
    {
        Xcp_SetErrorCode(XCP_ERR_PGM_ACTIVE);
        Xcp_RespLength = 0x02u;
    }
    else
    #endif
    /* check protection status */
    if (XCP_PL_CAL == (Xcp_ProtectionStatus & XCP_PL_CAL))
    {
        Xcp_SetErrorCode(XCP_ERR_ACCESS_LOCKED);
        Xcp_RespLength = 0x02u;
    }
    else if (segNum >= Xcp_SegmentInfo.maxSegNum)
    {
        Xcp_SetErrorCode(XCP_ERR_SEGMENT_NOT_VALID);
        Xcp_RespLength = 0x02u;
    }
    else
    {
        switch(accessMode)
        {
            case XCP_GET_PAG_MODE_ECU:
            case XCP_GET_PAG_MODE_XCP:
            {
                Xcp_RespBuffer[3] = Xcp_ActivPagNum[segNum];
                Xcp_RespLength = 4;
                break;
            }
            default:
            {
                Xcp_SetErrorCode(XCP_ERR_MODE_NOT_VALID);
                Xcp_RespLength = 0x02u;
                break;
            }
        }
    }
    Xcp_SendResp();
    return;
}
#endif

#endif /* (XCP_PAG_SUPPORT == STD_ON) */

#endif /* (XCP_PL_CAL == (XCP_PL_CAL & XCP_RESOURCE)) */

/*=======[E N D   O F   F I L E]==============================================*/

